#pragma once
#include "motion/rtcmds.h"
#include "zuc/nml_intf/zucpos.h"

#define DYN_CMD_CLASS_NAME(x) CMD_CLASS_NAME(x) "[DYN CMD]"

class CmdSetIdentifyCycleTime : public mot::MultiRobCmd
{
public:
    CmdSetIdentifyCycleTime(int grp_id, int identify_cycle_time)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetIdentifyCycleTime);
        identify_cycle_time_ = identify_cycle_time;
    };
    ~CmdSetIdentifyCycleTime() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int identify_cycle_time_;
};

class DynCmdStopDynamicsIdentify : public mot::MultiRobCmd
{
public:
    DynCmdStopDynamicsIdentify(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(DynCmdStopDynamicsIdentify);
    };
    ~DynCmdStopDynamicsIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_compensate_[9];
};

class DynCmdResetDynamicsIdentify : public mot::MultiRobCmd
{
public:
    DynCmdResetDynamicsIdentify(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(DynCmdResetDynamicsIdentify);
    };
    ~DynCmdResetDynamicsIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_compensate_[9];
};

class DynCmdInCalcDynamicsIdentify : public mot::MultiRobCmd
{
public:
    DynCmdInCalcDynamicsIdentify(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(DynCmdInCalcDynamicsIdentify);
    };
    ~DynCmdInCalcDynamicsIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_compensate_[9];
};

class DynCmdCompleteDynamicsIdentify : public mot::MultiRobCmd
{
public:
    DynCmdCompleteDynamicsIdentify(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(DynCmdCompleteDynamicsIdentify);
    };
    ~DynCmdCompleteDynamicsIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_compensate_[9];
};

class DynCmdSetDynamicsIdentifyTraj : public mot::MultiRobCmd
{
public:
    DynCmdSetDynamicsIdentifyTraj(int grp_id, const double* traj)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(DynCmdSetDynamicsIdentifyTraj);
        for (int i = 0; i < 7 * 7 * 4; i++) { dynamics_identify_traj_para_[i] = traj[i]; }
    };
    ~DynCmdSetDynamicsIdentifyTraj() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double dynamics_identify_traj_para_[7 * 7 * 4];
};

class CmdStopIdentifyFriction : public mot::MultiRobCmd
{
public:
    CmdStopIdentifyFriction(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdStopIdentifyFriction);
    };
    ~CmdStopIdentifyFriction() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int VARNAME;
};

class CmdStartIdentifyFriction : public mot::MultiRobCmd
{
public:
    CmdStartIdentifyFriction(int grp_id, int friction_identify_joint_index)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdStartIdentifyFriction);
        friction_identify_joint_index_ = friction_identify_joint_index;
    };
    ~CmdStartIdentifyFriction() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_identify_joint_index_;
};

class CmdSetIdentifyFrictionEndPos : public mot::MultiRobCmd
{
public:
    CmdSetIdentifyFrictionEndPos(int grp_id, int friction_identify_joint_index, const double* friction_identify_end_pos)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetIdentifyFrictionEndPos);
        friction_identify_joint_index_ = friction_identify_joint_index;
        for (int i = 0; i < 9; i++) { friction_identify_end_pos_[i] = friction_identify_end_pos[i]; }
    };
    ~CmdSetIdentifyFrictionEndPos() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_identify_joint_index_;
    double friction_identify_end_pos_[9];
};

class CmdSetIdentifyFrictionStartPos : public mot::MultiRobCmd
{
public:
    CmdSetIdentifyFrictionStartPos(int grp_id, int friction_identify_joint_index, const double* friction_identify_start_pos)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetIdentifyFrictionStartPos);
        friction_identify_joint_index_ = friction_identify_joint_index;
        for (int i = 0; i < 9; i++) { friction_identify_start_pos_[i] = friction_identify_start_pos[i]; }
    };
    ~CmdSetIdentifyFrictionStartPos() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int friction_identify_joint_index_;
    double friction_identify_start_pos_[9];
};

class CmdCompleteFrictionIdentify : public mot::MultiRobCmd
{
public:
    CmdCompleteFrictionIdentify(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdCompleteFrictionIdentify);
    };
    ~CmdCompleteFrictionIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdInFrictionIdentifyCalculating : public mot::MultiRobCmd
{
public:
    CmdInFrictionIdentifyCalculating(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdInFrictionIdentifyCalculating);
    };
    ~CmdInFrictionIdentifyCalculating() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int VARNAME;
};

class CmdSetServoDynamicsPara : public mot::MultiRobCmd
{
public:
    CmdSetServoDynamicsPara(int grp_id, const int* para)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetServoDynamicsPara);
        for (int i = 0; i < 128; i++) { para_[i] = para[i]; }
    };
    ~CmdSetServoDynamicsPara() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int para_[128];
};

class CmdSetDragControlEnable : public mot::MultiRobCmd
{
public:
    CmdSetDragControlEnable(int grp_id, int drag_control_enable)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetDragControlEnable);
        drag_control_enable_ = drag_control_enable;
    };
    ~CmdSetDragControlEnable() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int drag_control_enable_;
};

class CmdSetDragControlFcCompensate : public mot::MultiRobCmd
{
public:
    CmdSetDragControlFcCompensate(int grp_id, const int* drag_control_fc_compensate)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetDragControlFcCompensate);
        for (int i = 0; i < 9; i++) { drag_control_fc_compensate_[i] = drag_control_fc_compensate[i]; }
    }
    ~CmdSetDragControlFcCompensate() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int drag_control_fc_compensate_[9];
};

class CmdSetDragControlFvCompensate : public mot::MultiRobCmd
{
public:
    CmdSetDragControlFvCompensate(int grp_id, const int* drag_control_fv_compensate)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetDragControlFvCompensate);
        for (int i = 0; i < 9; i++) { drag_control_fv_compensate_[i] = drag_control_fv_compensate[i]; }
    };
    ~CmdSetDragControlFvCompensate() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int drag_control_fv_compensate_[9];
};

class CmdSetDragControlFilterFactor : public mot::MultiRobCmd
{
public:
    CmdSetDragControlFilterFactor(int grp_id, const int* drag_control_filter_factor)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetDragControlFilterFactor);
        for (int i = 0; i < 9; i++) { drag_control_filter_factor_[i] = drag_control_filter_factor[i]; }
    };
    ~CmdSetDragControlFilterFactor() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int drag_control_filter_factor_[9];
};

class CmdSetDragControlDitherFactor : public mot::MultiRobCmd
{
public:
    CmdSetDragControlDitherFactor(int grp_id, const double* drag_control_dither_factor)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetDragControlDitherFactor);
        for (int i = 0; i < 9; i++) { drag_control_dither_factor_[i] = drag_control_dither_factor[i]; }
    };
    ~CmdSetDragControlDitherFactor() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double drag_control_dither_factor_[9];
};

class CmdSetGravityDirection : public mot::MultiRobCmd
{
public:
    CmdSetGravityDirection(int grp_id, const double* rpy)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetGravityDirection);
        for (int i = 0; i < 3; i++) { rpy_[i] = rpy[i]; }
    };
    ~CmdSetGravityDirection() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double rpy_[3];
};

class CmdSetCollisionGmRecordEnable : public mot::MultiRobCmd
{
public:
    CmdSetCollisionGmRecordEnable(int grp_id, double gm_record_enable)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetCollisionGmRecordEnable);
        gm_record_enable_ = gm_record_enable;
    };
    ~CmdSetCollisionGmRecordEnable() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double gm_record_enable_;
};

class CmdSetCollisionGmThreshold : public mot::MultiRobCmd
{
public:
    CmdSetCollisionGmThreshold(int grp_id, const double* gm_threshold)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetCollisionGmThreshold);
        for (int i = 0; i < 9; i++) { gm_threshold_[i] = gm_threshold[i]; }
    };
    ~CmdSetCollisionGmThreshold() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double gm_threshold_[9];
};

class CmdSetCollisionFilterPara : public mot::MultiRobCmd
{
public:
    CmdSetCollisionFilterPara(int grp_id, double gm_filter)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdSetCollisionFilterPara);
        gm_filter_ = gm_filter;
    };
    ~CmdSetCollisionFilterPara() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double gm_filter_;
};

class CmdStartPayloadIdentify : public mot::MultiRobCmd
{
public:
    CmdStartPayloadIdentify(int grp_id, int withpayload, const JointTraj& jointTraj, int id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdStartPayloadIdentify);
        withPayload_ = withpayload;
        jointTraj_ = jointTraj;
        id_ = id;
    };
    ~CmdStartPayloadIdentify() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    JointTraj jointTraj_;
    int withPayload_;
};

class CmdResetIdentifyStat : public mot::MultiRobCmd
{
public:
    CmdResetIdentifyStat(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = DYN_CMD_CLASS_NAME(CmdResetIdentifyStat);
    };
    ~CmdResetIdentifyStat() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};